/*
This file is part of MMM.

MMM is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.

MMM is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with MMM.  If not, see <http://www.gnu.org/licenses/>.
*
* @package    MMM
* @author     Andre Meixner
* @copyright  2017 High Performance Humanoid Technologies (H2T), Karlsruhe, Germany
*
*/

#include <MMM/Motion/Motion.h>
#include <MMM/Motion/MotionWriterXML.h>
#include <MMM/Motion/MotionReaderC3D.h>

#include "MMMC3DConverterConfiguration.h"
#include "C3DConverter.h"

#include <string>
#include <vector>
#include <tuple>

using namespace MMM;

int main(int argc, char *argv[]) {
    MMM_INFO << " --- MMMC3DConverter --- " << endl;
    MMMC3DConverterConfiguration* configuration = new MMMC3DConverterConfiguration();
    if (!configuration->processCommandLine(argc, argv)) {
        MMM_ERROR << "Error while processing command line, aborting..." << endl;
        return -1;
    }

    std::vector<std::tuple<std::string, std::string> > motionNameToMarkerPrefix;
    for (unsigned int i = 0; i < configuration->motionNames.size(); i++) {
        if (configuration->motionNames[i].empty()) {
            configuration->motionNames[i] = "Motion_" +std::to_string(i);
        }
        else {
            for (const auto &mapping : motionNameToMarkerPrefix) {
                if (std::get<0>(mapping) == configuration->motionNames[i]) {
                    MMM_ERROR << "Motion name '" + configuration->motionNames[i] + "' for the marker prefix '" + configuration->markerPrefix[i] + "' is already contained!" << endl;
                    return -3;
                }
            }
            motionNameToMarkerPrefix.push_back(std::make_tuple(configuration->motionNames[i], configuration->markerPrefix[i]));
        }
    }

    try {
        MMM_INFO << "Reading c3d motion file..." << std::endl;
        MMM::C3DConverterPtr converter(new MMM::C3DConverter(configuration->inputMotionPath));

        // use c3d prefixes as motion names
        if (motionNameToMarkerPrefix.size() == 0) {
            for (const auto &prefix : converter->getPrefixMarkerData()->getPrefixes()) {
                motionNameToMarkerPrefix.push_back(std::make_tuple<>(prefix, prefix));
            }
        }

        MMM_INFO << "Converting motions..." << std::endl;
        MMM::MotionList motions = converter->convertMotions(motionNameToMarkerPrefix, configuration->sensorName, configuration->sensorDescription);

        MMM_INFO << "Writing motions to " << configuration->outputMotionPath << std::endl;
        MMM::MotionWriterXML::writeMotion(motions, configuration->outputMotionPath);
    } catch (MMM::Exception::MMMException& e) {
        MMM_ERROR << e.what() << endl;
        return -4;
    }
}
